﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.ComponentModel;
using System.Drawing;
using System.Globalization;
using System.Linq;
using System.Reflection;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearRegression;
using MathNet.Numerics.Statistics;
using ScottPlot;

namespace 调平
{
    internal class Program
    {
        private static void Main()
        {
            /*
             * 方法1
             */
            //数据1
            #region
            // Top Chuck 传感器数据
            Dictionary<string, double> topChuckData = new Dictionary<string, double>
                {
                    {"In0_1", 23.84770966}, {"In0_2", -6.376930237}, {"In0_3", -0.194458008}, {"Mid0", 1.028503418},
                    {"In45_1", -3.536563873}, {"In45_2", -0.87474823}, {"In45_3", 0.700706482}, {"Mid45", -1.003303528},
                    {"In90_1", -0.985961914}, {"In90_2", -0.574142456}, {"In90_3", -4.267082214}, {"Mid90", -1.469314575},
                    {"In135_1", 0.387619019}, {"In135_2", -0.473937988}, {"In135_3", 1.510803223}, {"Mid135", 0.003700256},
                    {"In180_1", 24.58776855}, {"In180_2", -1.974632263}, {"In180_3", -0.345870972}, {"Mid180", -0.363586426},
                    {"In225_1", -1.766098022}, {"In225_2", 0.067398071}, {"In225_3", -0.177062988}, {"Mid225", -3.872184753},
                    {"In270_1", 0.21295166}, {"In270_2", -3.948898315}, {"In270_3", -2.954559326}, {"Mid270", -1.540847778},
                    {"In315_1", -0.123123169}, {"In315_2", -3.276229858}, {"In315_3", 0.038719177}, {"Mid315", 0.487678528}
                };

            // Bottom Chuck 传感器数据
            Dictionary<string, double> bottomChuckData = new Dictionary<string, double>()
            {
                {"In0_1", 359.831512451172}, {"In0_2", 334.982208251953}, {"In0_3", 343.849426269531}, {"Mid0", 359.376770019531},
                {"In45_1", 325.485046386719}, {"In45_2", 331.0009765625}, {"In45_3", 333.58203125}, {"Mid45", 340.094177246094},
                {"In90_1", 318.524810791016}, {"In90_2", 317.856384277344}, {"In90_3", 317.488647460938}, {"Mid90", 310.48779296875},
                {"In135_1", 313.2939453125}, {"In135_2", 309.127075195313}, {"In135_3", 305.838592529297}, {"Mid135", 293.650817871094},
                {"In180_1", 332.613403320313}, {"In180_2", 307.51611328125}, {"In180_3", 305.146362304688}, {"Mid180", 292.283447265625},
                {"In225_1", 318.799835205078}, {"In225_2", 318.90673828125}, {"In225_3", 317.337341308594}, {"Mid225", 314.568176269531},
                {"In270_1", 329.658203125}, {"In270_2", 329.308685302734}, {"In270_3", 331.176422119141}, {"Mid270", 329.966400146484},
                {"In315_1", 337.61474609375}, {"In315_2", 339.063934326172}, {"In315_3", 345.628540039063}, {"Mid315", 360.277770996094}
            };

            //数据2  结果验证了这个程序中的算法和我们fusion 中的算法是一致的。
            // Top Chuck 传感器数据（更新后）
            //    Dictionary<string, double> topChuckData = new Dictionary<string, double>
            //{
            //    {"In0_1", 23.88535309}, {"In0_2", -6.987937927}, {"In0_3", -0.121818542}, {"Mid0", 0.472518921},
            //    {"In45_1", -3.774833679}, {"In45_2", -0.929153442}, {"In45_3", -0.307624817}, {"Mid45", -1.194900513},
            //    {"In90_1", -0.981132507}, {"In90_2", -0.651473999}, {"In90_3", -4.496459961}, {"Mid90", -1.926849365},
            //    {"In135_1", 0.235412598}, {"In135_2", -0.670013428}, {"In135_3", 2.152877808}, {"Mid135", -0.452095032},
            //    {"In180_1", 10.51000977}, {"In180_2", -2.239189148}, {"In180_3", -0.419754028}, {"Mid180", -0.797561646},
            //    {"In225_1", -2.41116333}, {"In225_2", 0.091491699}, {"In225_3", -0.514709473}, {"Mid225", -4.087341309},
            //    {"In270_1", -27.11665344}, {"In270_2", -5.115524292}, {"In270_3", -2.630455017}, {"Mid270", 0.146575928},
            //    {"In315_1", -0.800567627}, {"In315_2", -3.065811157}, {"In315_3", -0.275238037}, {"Mid315", -0.052696228}
            //};

            //    // Bottom Chuck 传感器数据（更新后）
            //    Dictionary<string, double> bottomChuckData = new Dictionary<string, double>
            //{
            //    {"In0_1", 357.5815125}, {"In0_2", 332.3037109}, {"In0_3", 341.5246277}, {"Mid0", 356.9660645},
            //    {"In45_1", 323.3222656}, {"In45_2", 328.7756042}, {"In45_3", 331.3685913}, {"Mid45", 338.0082397},
            //    {"In90_1", 316.5126038}, {"In90_2", 315.7456665}, {"In90_3", 315.415741}, {"Mid90", 308.4900513},
            //    {"In135_1", 311.2686157}, {"In135_2", 307.0535583}, {"In135_3", 303.7873535}, {"Mid135", 291.7260132},
            //    {"In180_1", 318.3375854}, {"In180_2", 305.3764648}, {"In180_3", 303.2308044}, {"Mid180", 290.2286987},
            //    {"In225_1", 316.5319824}, {"In225_2", 316.7035828}, {"In225_3", 315.1881714}, {"Mid225", 312.3748779},
            //    {"In270_1", 307.3053894}, {"In270_2", 327.0389404}, {"In270_3", 328.885498}, {"Mid270", 327.5952454},
            //    {"In315_1", 335.2342834}, {"In315_2", 336.7516785}, {"In315_3", 343.2346802}, {"Mid315", 357.8313599}
            //};

            #region testcode

            //double[] radii = { 45000, 60000, 72500, 120000 }; // 对应传感器 1-4 的半径

            //// 传感器角度（度）
            //double[] angles = { 0, 45, 90, 135, 180, 225, 270, 315 };
            //// 计算高度差 (bottom - top)
            //Dictionary<string, double> heightDiffs = new Dictionary<string, double>();
            //foreach (var key in topChuckData.Keys)
            //{
            //    heightDiffs[key] = bottomChuckData[key] - topChuckData[key];
            //}

            //// 构建最小二乘法矩阵 A 和向量 b
            //List<double[]> A_rows = new List<double[]>();
            //List<double> b_values = new List<double>();

            //foreach (double angle in angles)
            //{
            //    for (int sensorIdx = 0; sensorIdx < 4; sensorIdx++)
            //    {
            //        string key = $"In{angle}_{sensorIdx + 1}";
            //        if (!heightDiffs.ContainsKey(key)) continue;

            //        double r = radii[sensorIdx];
            //        double x1 = r * Math.Cos(angle * Math.PI / 180.0); // x 坐标 (mm)
            //        double y = r * Math.Sin(angle * Math.PI / 180.0); // y 坐标 (mm)
            //        double z = heightDiffs[key];                      // 高度差

            //        A_rows.Add(new double[] { x1, y, 1 }); // 平面方程: z = a*x + b*y + c
            //        b_values.Add(z);
            //    }
            //}

            //// 转换为 MathNet.Numerics 的矩阵
            //Matrix<double> A = Matrix<double>.Build.DenseOfRowArrays(A_rows);
            //Vector<double> b = Vector<double>.Build.Dense(b_values.ToArray());

            //// 最小二乘法求解 (Ax = b)
            //Vector<double> x = A.Solve(b);

            //// 提取 rx, ry (转换为微弧度)
            //double a = x[0]; // x 方向斜率 (µm/mm 或 mrad)
            //double b_coeff = x[1]; // y 方向斜率 (µm/mm 或 mrad)
            //double rx = b_coeff * 1000; // 转换为 µrad
            //double ry = a * 1000;       // 转换为 µrad

            //Console.WriteLine($"Bottom Chuck 需要调整的 rx (µrad): {rx:F2}");
            //Console.WriteLine($"Bottom Chuck 需要调整的 ry (µrad): {ry:F2}");

            #endregion testcode

            List<Point3D> pointsList00 = new List<Point3D>();
            List<double> zList = new List<double>();
            foreach (var b1 in bottomChuckData)
            {
                foreach (var t1 in topChuckData)
                {
                    if (b1.Key == t1.Key)
                    {
                        zList.Add(b1.Value - t1.Value);
                    }
                }
            }
            zList = new List<double>()
                                {
                                345.837287902832,351.232917785645,353.808868408203,368.067749023438,338.8518409729,341.688774108887,342.686317443848,350.85669708252,329.370697021484,328.254623413086,331.63842010498,321.69465637207,322.78108215332,319.472869873047,314.20556640625,303.529563903809,317.937316894531,319.408714294434,315.45573425293,302.609558105469,330.483963012695,328.710372924805,327.386535644531,328.467216491699,339.302764892578,343.218032836914,344.060760498047,341.497085571289,347.549087524414,352.232192993164,355.449195861816,369.650871276855
                                };
            int num = 0;

            List<double> radiusList = new List<double>() { 45000, 60000, 72500, 120000 };
            List<double> angleList = new List<double>() { 0, 45, 90, 135, 180, 225, 270, 315 };

            for (int angleIndex = 0; angleIndex < angleList.Count(); angleIndex++)
            {
                for (int radiusIndex = 0; radiusIndex < radiusList.Count(); radiusIndex++)
                {
                    {
                        double x1 = radiusList[radiusIndex] * Math.Cos(angleList[angleIndex] / (180.0 / Math.PI));
                        double y = radiusList[radiusIndex] * Math.Sin(angleList[angleIndex] / (180.0 / Math.PI));
                        double z = zList[num++];//zList[radiusIndex * angleList.Count() + angleIndex];
                        pointsList00.Add(new Point3D(x1, y, z));
                    }
                }
            }

            var result = FitPlaneAngle(pointsList00, 2);
            Console.WriteLine($"{string.Join(",", result)}"); // 数据1 执行的结果不对，无法调平。正确的结果应该是rx：-118	ry：-273

            //var cResult = $"{string.Join(",", result)}" + " | Z：" + $"{string.Join(",", zList)} ";
            Console.ReadLine();
            #endregion
        }

        private static Dictionary<string, (double X, double Y)> GenerateSensorPositions()
        {
            var positions = new Dictionary<string, (double X, double Y)>();
            double[] radii = new double[] { 45000, 60000, 72500, 120000 };
            int[] angles = new int[] { 0, 45, 90, 135, 180, 225, 270, 315 };

            foreach (var deg in angles)
            {
                double angle = deg * Math.PI / 180.0;
                for (int j = 0; j < radii.Length; j++)
                {
                    double x = radii[j] * Math.Cos(angle);
                    double y = radii[j] * Math.Sin(angle);
                    positions[$"In{deg}_{j + 1}"] = (x, y);
                    if (j == 3)
                    {
                        positions[$"Mid{deg}"] = (x, y);
                    }
                }
            }
            return positions;
        }

        private static (double A, double B, double C, double Error) FitPlane(Dictionary<string, double> data, Dictionary<string, (double X, double Y)> sensorPos)
        {
            var points = new List<SensorData>();
            foreach (var item in data)
            {
                if (sensorPos.TryGetValue(item.Key, out var pos))
                {
                    points.Add(new SensorData
                    {
                        Name = item.Key,
                        X = pos.X,
                        Y = pos.Y,
                        Z = item.Value
                    });
                }
            }

            if (points.Count == 0)
                throw new ArgumentException("没有找到匹配的传感器数据");

            // 构建矩阵方程 A*[a,b,c]^T = z
            var x = Vector<double>.Build.Dense(points.Select(p => p.X).ToArray());
            var y = Vector<double>.Build.Dense(points.Select(p => p.Y).ToArray());
            var z = Vector<double>.Build.Dense(points.Select(p => p.Z).ToArray());
            var A = Matrix<double>.Build.DenseOfColumns(new[] { x, y, Vector<double>.Build.Dense(x.Count, 1) });

            // 最小二乘解
            var coeffs = A.TransposeThisAndMultiply(A).Inverse() * A.Transpose() * z;
            var a = coeffs[0];
            var b = coeffs[1];
            var c = coeffs[2];

            // 计算拟合误差
            var residuals = z - (a * x + b * y + c);
            var rmsError = Math.Sqrt(residuals.PointwisePower(2).Mean());

            return (a, b, c, rmsError);
        }

        private static (double RxMicroRad, double RyMicroRad) CalculateAdjustment(
            (double A, double B, double C, double Error) topPlane,
            (double A, double B, double C, double Error) bottomPlane)
        {
            double rxRad = Math.Atan(bottomPlane.A) - Math.Atan(topPlane.A);
            double ryRad = Math.Atan(bottomPlane.B) - Math.Atan(topPlane.B);

            return (rxRad * 1e6, ryRad * 1e6);
        }

        //private static void PlotResults(
        //    Dictionary<string, double> topData,
        //    Dictionary<string, double> bottomData,
        //    Dictionary<string, (double X, double Y)> sensorPos,
        //    (double A, double B, double C, double Error) topPlane,
        //    (double A, double B, double C, double Error) bottomPlane)
        //{
        //    // 创建Topchuck图表
        //    var topPlot = new Plot(800, 600);
        //    topPlot.Title("Topchuck 平面拟合");
        //    topPlot.XLabel("X Position");
        //    topPlot.YLabel("Y Position");

        //    // 添加Topchuck数据点
        //    var topPoints = new List<SensorData>();
        //    foreach (var item in topData)
        //    {
        //        if (sensorPos.TryGetValue(item.Key, out var pos))
        //        {
        //            topPoints.Add(new SensorData { X = pos.X, Y = pos.Y, Z = item.Value });
        //        }
        //    }
        //    topPlot.AddScatterPoints(
        //        topPoints.Select(p => p.X).ToArray(),
        //        topPoints.Select(p => p.Y).ToArray(),
        //        markerSize: 5, color: System.Drawing.Color.Blue, label: "Topchuck Data");

        //    // 创建Bottomchuck图表
        //    var bottomPlot = new Plot(800, 600);
        //    bottomPlot.Title("Bottomchuck 平面拟合");
        //    bottomPlot.XLabel("X Position");
        //    bottomPlot.YLabel("Y Position");

        //    // 添加Bottomchuck数据点
        //    var bottomPoints = new List<SensorData>();
        //    foreach (var item in bottomData)
        //    {
        //        if (sensorPos.TryGetValue(item.Key, out var pos))
        //        {
        //            bottomPoints.Add(new SensorData { X = pos.X, Y = pos.Y, Z = item.Value });
        //        }
        //    }
        //    bottomPlot.AddScatterPoints(
        //        bottomPoints.Select(p => p.X).ToArray(),
        //        bottomPoints.Select(p => p.Y).ToArray(),
        //        markerSize: 5, color: System.Drawing.Color.Red, label: "Bottomchuck Data");

        //    // 显示图表
        //    topPlot.Legend();
        //    bottomPlot.Legend();

        //    var topChart = new FormsPlotViewer(topPlot);
        //    topChart.Text = "Topchuck 平面拟合";

        //    var bottomChart = new FormsPlotViewer(bottomPlot);
        //    bottomChart.Text = "Bottomchuck 平面拟合";

        //    System.Windows.Forms.Application.Run(topChart);
        //    System.Windows.Forms.Application.Run(bottomChart);

        //    #endregion
        //}

        /// <summary>
        /// 弧度转角度
        /// </summary>
        /// <param name="data">输入所有的3D点数据</param>
        /// <param name="numMad">MAD过滤数据的阈值</param>
        /// <returns name="results">第一个：法向量与 X 轴负方向的夹角，单位:urad.第二个：法向量与 Y 轴负方向的夹角，单位:urad.第三个：过滤点跟拟合平面的Z偏差总和</returns>
        public static double[] FitPlaneAngle(List<Point3D> data, double numMad = 2)
        {
            double[] results = new double[3] { 0, 0, 0 };
            List<double> zList = new List<double>();
            foreach (var item in data)
            {
                zList.Add(item.Z);
            }
            var middle = Median(zList.ToArray());
            var mad = CalculateMAD(zList.ToArray());
            var pointsListFilter = data.Where(t => t.Z - middle <= numMad * mad).ToList();
            var planeParam = FitPlane(pointsListFilter);

            double angleRx = Math.Asin(planeParam[0]) * 1E6;
            double angleRy = Math.Asin(planeParam[1]) * 1E6;
            double zOffsetSum = 0.0;
            for (int indexNum = 0; indexNum < pointsListFilter.Count(); indexNum++)
            {
                double offset = pointsListFilter[indexNum].Z - (planeParam[0] * pointsListFilter[indexNum].X + planeParam[1] * pointsListFilter[indexNum].Y + planeParam[2]);
                zOffsetSum += Math.Abs(offset);
            }
            results[0] = angleRy;
            results[1] = angleRx;
            results[2] = zOffsetSum / pointsListFilter.Count();
            return results;
        }

        private static double Median(double[] data)
        {
            data = data.OrderBy(n => n).ToArray();
            int middle = data.Length / 2;
            if (data.Length % 2 == 0)
                return (data[middle] + data[middle - 1]) / 2.0;
            else
                return data[middle];
        }

        private static double CalculateMAD(double[] data)
        {
            double median = Median(data);
            double[] absoluteDeviations = data.Select(x => Math.Abs(x - median)).ToArray();
            double medianAbsoluteDeviation = Median(absoluteDeviations);
            return medianAbsoluteDeviation;
        }

        public static double[] FitPlane(List<Point3D> pointsList)
        {
            double[] planeCoefficient = new double[3];
            int pointNum = pointsList.Count();
            if (pointNum < 3)
            {
                return planeCoefficient;
            }
            double[] point_X = new double[pointNum];
            double[] point_Y = new double[pointNum];
            double[] point_Z = new double[pointNum];
            for (int i = 0; i < pointNum; i++)
            {
                point_X[i] = pointsList[i].X;
                point_Y[i] = pointsList[i].Y;
                point_Z[i] = pointsList[i].Z;
            }
            Matrix<double> A = Matrix<double>.Build.Dense(pointNum, 3);
            Vector<double> B = Vector<double>.Build.Dense(pointNum);
            for (int i = 0; i < pointNum; i++)
            {
                A[i, 0] = point_X[i];
                A[i, 1] = point_Y[i];
                A[i, 2] = 1;
                B[i] = point_Z[i];
            }

            var result = MultipleRegression.QR(A, B);
            planeCoefficient[0] = result[0];
            planeCoefficient[1] = result[1];
            planeCoefficient[2] = result[2];
            return planeCoefficient;
        }
    }

    internal class SensorData
    {
        public string Name { get; set; }
        public double X { get; set; }
        public double Y { get; set; }
        public double Z { get; set; }
    }

    public struct Point3D
    {
        public static readonly PointD Empty;

        public double X { get; set; }

        public double Y { get; set; }

        public double Z { get; set; }

        public bool IsEmpty
        {
            get
            {
                return X == 0 && Y == 0 && Z == 0;
            }
        }

        public Point3D(double x, double y, double z)
        {
            this.X = x;
            this.Y = y;
            this.Z = z;
        }

        public static Point3D operator *(Point3D pt1, double a)
        {
            return new Point3D(pt1.X * a, pt1.Y * a, pt1.Z * a);
        }

        public static Point3D operator *(Point3D pt1, Point3D pt2)
        {
            return new Point3D(pt1.X * pt2.X, pt1.Y * pt2.Y, pt1.Z * pt2.Z);
        }

        public static Point3D operator /(Point3D pt1, Point3D pt2)
        {
            return new Point3D(pt1.X / pt2.X, pt1.Y / pt2.Y, pt1.Z / pt2.Z);
        }

        public static Point3D operator +(Point3D pt1, Point3D pt2)
        {
            return Add(pt1, pt2);
        }

        public static Point3D operator -(Point3D pt1, Point3D pt2)
        {
            return Subtract(pt1, pt2);
        }

        public static Point3D Add(Point3D pt1, Point3D pt2)
        {
            return new Point3D(pt1.X + pt2.X, pt1.Y + pt2.Y, pt1.Z + pt2.Z);
        }

        public static Point3D Subtract(Point3D pt1, Point3D pt2)
        {
            return new Point3D(pt1.X - pt2.X, pt1.Y - pt2.Y, pt1.Z - pt2.Z);
        }

        public double Length()
        {
            return Math.Sqrt(this.X * this.X + this.Y * this.Y + this.Z * this.Z);
        }

        public double DistanceTo(Point3D p)
        {
            return Math.Sqrt((p.X - X) * (p.X - X) + (p.Y - Y) * (p.Y - Y) + (p.Z - Z) * (p.Z - Z));
        }
    }

    /// <summary>
    /// double型的Point结构，避免精度丢失
    /// </summary>
    [Serializable]
    [TypeConverter(typeof(ExpandableObjectConverter))]
    public struct PointD
    {
        public static readonly PointD Empty;

        public double X { get; set; }

        public double Y { get; set; }

        [Browsable(false)]
        public bool IsEmpty
        {
            get
            {
                return X == 0 && Y == 0;
            }
        }

        public PointD(double x, double y)
        {
            this.X = x;
            this.Y = y;
        }

        public static PointD operator *(PointD pt1, double a)
        {
            return new PointD(pt1.X * a, pt1.Y * a);
        }

        public static PointD operator *(PointD pt1, PointD pt2)
        {
            return new PointD(pt1.X * pt2.X, pt1.Y * pt2.Y);
        }

        public static PointD operator /(PointD pt1, PointD pt2)
        {
            return new PointD(pt1.X / pt2.X, pt1.Y / pt2.Y);
        }

        public static PointD operator +(PointD pt1, PointD pt2)
        {
            return Add(pt1, pt2);
        }

        public static PointD operator -(PointD pt1, PointD pt2)
        {
            return Subtract(pt1, pt2);
        }

        public static bool operator ==(PointD left, PointD right)
        {
            if (left.X == right.X)
            {
                return left.Y == right.Y;
            }
            return false;
        }

        public static bool operator !=(PointD left, PointD right)
        {
            return !(left == right);
        }

        public static PointD Add(PointD pt1, PointD pt2)
        {
            return new PointD(pt1.X + pt2.X, pt1.Y + pt2.Y);
        }

        public static PointD Subtract(PointD pt1, PointD pt2)
        {
            return new PointD(pt1.X - pt2.X, pt1.Y - pt2.Y);
        }

        //PointF  -> PointD
        public static implicit operator PointD(PointF f)
        {
            var tmp = new PointD();
            tmp.X = f.X;
            tmp.Y = f.Y;
            return tmp;
        }

        //PointD -> PointF
        public static implicit operator PointF(PointD d)
        {
            var tmp = new PointF();
            tmp.X = (float)d.X;
            tmp.Y = (float)d.Y;
            return tmp;
        }

        public override bool Equals(object obj)
        {
            if (!(obj is PointD))
            {
                return false;
            }
            PointD pointD = (PointD)obj;
            if (pointD.X == X && pointD.Y == Y)
            {
                return true;
            }
            return false;
        }

        public override int GetHashCode()
        {
            return base.GetHashCode();
        }

        public string ToString(uint decimalPlace)
        {
            return string.Format(CultureInfo.CurrentCulture, "{0:F" + decimalPlace + "},{1:F" + decimalPlace + "}", new object[2] { X, Y });
        }

        public override string ToString()
        {
            return string.Format(CultureInfo.CurrentCulture, "{0:F6},{1:F6}", new object[2] { X, Y });
        }

        public PointD Round(int decimals)
        {
            return new PointD(Math.Round(X, decimals), Math.Round(Y, decimals));
        }

        public double DistanceTo(PointD p)
        {
            return Math.Sqrt((p.X - X) * (p.X - X) + (p.Y - Y) * (p.Y - Y));
        }
    }
}